classdef AccIncErrTest < matlab.unittest.TestCase
    
    methods (Test)
        function vldAccLevArmAndAnisoErr_inc(testCase)
            % 验证增量形式与连续形式的杆臂与不等惯量误差的等效性
            
            % 参数设置
            nSamp = 1000;
            Tl = 0.005; % 5ms
            omegaIBB0 = randi([-100, 100], [3 nSamp]) / 180 * pi; % 100°/s
            omegaIBBRate = randi([-200, 200], [3 nSamp]) / 180 * pi; % 200°/s^2
            IMUPar = genrandimupar(nSamp, false);
            
            dfSizeSIntTrue = NaN(3, nSamp);
            dfSizeSInt = NaN(3, nSamp);
            dfAnisoSIntTrue = NaN(3, nSamp);
            dfAnisoSInt = NaN(3, nSamp);
            for i=1:nSamp
                DalphaBl = integral(@(t)(omegaIBB0(:, i)+t*omegaIBBRate(:, i)), 0, Tl, 'ArrayValued', true);
                DalphaBl1 = integral(@(t)(omegaIBB0(:, i)+t*omegaIBBRate(:, i)), -Tl, 0, 'ArrayValued', true);
                
                PSBT = (IMUPar.acc.PS0B(:, :, i)*(eye(3)+IMUPar.acc.dPSS0(:, :, i)))';
                dfSizeSIntTrue(:, i) = integral(@(t)acclevarmerr_cont(PSBT, IMUPar.acc.lB(:, :, i), ...
                    omegaIBB0(:, i)+t*omegaIBBRate(:, i), omegaIBBRate(:, i)), ...
                    0, Tl, 'ArrayValued', true);
                dfSizeSInt(:, i) = acclevarmerr_inc(PSBT, IMUPar.acc.lB(:, :, i), DalphaBl, DalphaBl1, Tl);
                
                dfAnisoSIntTrue(:, i) = integral(@(t)accanisoerr_cont(IMUPar.acc.KAniso(:, i), IMUPar.acc.CBA(:, :, :, i), omegaIBB0(:, i)+t*omegaIBBRate(:, i)), ...
                    0, Tl, 'ArrayValued', true);
                dfAnisoSInt(:, i) = accanisoerr_inc(IMUPar.acc.KAniso(:, i), IMUPar.acc.CBA(:, :, :, i), DalphaBl, DalphaBl1, Tl);
            end
            
            dfSizeSIntErr = (dfSizeSInt - dfSizeSIntTrue)' / 9.8 * 1e6 / Tl;
            disp('dfSizeSInt误差绝对值最大值（μg）'); % 应为可以忽略的量级
            disp(max(abs(dfSizeSIntErr), [], 1));
            disp('dfSizeSInt误差绝对值平均值（μg）'); % 应为可以忽略的量级
            disp(mean(abs(dfSizeSIntErr), 1));
            testCase.verifyLessThanOrEqual(max(abs(dfSizeSIntErr)), 5e-9);
            testCase.verifyLessThanOrEqual(mean(abs(dfSizeSIntErr)), 5e-10);
            
            dfAnisoSIntErr = (dfAnisoSInt - dfAnisoSIntTrue)' / 9.8 * 1e6 / Tl;
            disp('dfAnisoSInt误差绝对值最大值（μg）'); % 应为可以忽略的量级
            disp(max(abs(dfAnisoSIntErr), [], 1));
            disp('dfAnisoSInt误差绝对值平均值（μg）'); % 应为可以忽略的量级
            disp(mean(abs(dfAnisoSIntErr), 1));
            testCase.verifyLessThanOrEqual(max(abs(dfAnisoSIntErr)), 1.5e-12);
            testCase.verifyLessThanOrEqual(mean(abs(dfAnisoSIntErr)), 1.5e-13);
        end
    end
end